#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
PID controller implementation adapted from legacy src/utils/pid_controller.py.
"""

from __future__ import annotations

from dataclasses import replace
from typing import Optional

from ...config.settings import PidAxisConfig


class PIDController:
    """
    基础 PID 控制器。
    """

    def __init__(
        self,
        kp: float = 0.6,
        ki: float = 0.0,
        kd: float = 0.0,
        max_integral: float = 20.0,
        first_update_factor: float = 0.3,
        output_limit: float = 100.0,
        integral_error_threshold: float = 10.0,
    ):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.max_integral = max_integral
        self.first_update_factor = first_update_factor
        self.output_limit = output_limit
        self.integral_error_threshold = integral_error_threshold

        self.prev_error = 0.0
        self.integral = 0.0
        self.first_update = True

    @classmethod
    def from_config(cls, axis_config: Optional[PidAxisConfig]) -> "PIDController":
        """
        通过配置创建控制器。
        """
        if axis_config is None:
            return cls()

        cfg = replace(axis_config)  # copy to avoid mutation
        return cls(
            kp=cfg.kp,
            ki=cfg.ki,
            kd=cfg.kd,
            max_integral=cfg.max_integral,
            first_update_factor=cfg.first_update_factor,
            output_limit=cfg.output_limit,
            integral_error_threshold=cfg.integral_error_threshold,
        )

    def update(self, error: float, dt: float) -> float:
        """
        更新 PID。
        """
        if self.first_update:
            self.prev_error = error
            self.integral = 0.0
            self.first_update = False
            return self.kp * error * self.first_update_factor

        if abs(error) < self.integral_error_threshold:
            self.integral += error * dt
        else:
            self.integral += error * dt * 0.3

        self.integral = max(-self.max_integral, min(self.max_integral, self.integral))

        derivative = (error - self.prev_error) / max(dt, 1e-3)

        output = self.kp * error + self.ki * self.integral + self.kd * derivative
        output = max(-self.output_limit, min(self.output_limit, output))

        self.prev_error = error
        return output

    def reset(self) -> None:
        """重置控制器状态。"""
        self.prev_error = 0.0
        self.integral = 0.0
        self.first_update = True
